#include <blackbox/blackbox.h>

#ifdef BUOLA_HAVE_DC1394

#include <buola/image.h>

#include "cdc1394camera.h"
#include "cdc1394.h"

namespace buola { namespace blackbox {

CDC1394Camera::CDC1394Camera(CDC1394 *pDC,dc1394camera_t *pCam)
    :   mDC(pDC)
    ,   mCam(pCam)
    ,   mIsInit(false)
    ,   mIsStarted(false)
{
}

CDC1394Camera::~CDC1394Camera()
{
    Stop();
    dc1394_camera_free(mCam);
}

void CDC1394Camera::Initialize(bool pCleanupChannels)
{
    if(mIsInit) return;
    dc1394_camera_reset(mCam);

    if(pCleanupChannels)
        dc1394_iso_release_all(mCam);

    if(dc1394_feature_get_all(mCam,&mFeatureSet)!=DC1394_SUCCESS)
        throw XHardware("unable to get feature set");

    if(dc1394_video_get_supported_modes(mCam,&mModes)!=DC1394_SUCCESS)
        throw XHardware("unable to get video modes");

    if(dc1394_video_get_iso_speed(mCam,&mISOSpeed)!=DC1394_SUCCESS)
        throw XHardware("unable to get iso speed");

    if(dc1394_video_get_mode(mCam,&mMode)!=DC1394_SUCCESS)
        throw XHardware("unable to get video mode");

    if(dc1394_video_get_framerate(mCam,&mFrameRate)!=DC1394_SUCCESS)
        throw XHardware("unable to get framerate");

    mBayerFilter=DC1394_COLOR_FILTER_GBRG;

    mIsInit=true;
}

void CDC1394Camera::Start()
{
    if(mIsStarted) return;

    if(dc1394_capture_setup(mCam,10,DC1394_CAPTURE_FLAGS_DEFAULT)!=DC1394_SUCCESS)
    {
        throw XHardware("unable to set up capture");
    }

    if (dc1394_video_set_transmission(mCam,DC1394_ON)!=DC1394_SUCCESS)
    {
        throw XHardware("unable to start camera iso transmission");
    }

    mIsStarted=true;
}

void CDC1394Camera::Stop()
{
    if(!mIsStarted) return;

    if (dc1394_video_set_transmission(mCam,DC1394_OFF)!= DC1394_SUCCESS)
    {
        throw XHardware("couldn't stop the camera");
    }

    dc1394_iso_release_all(mCam);
    dc1394_capture_stop(mCam);

    mIsStarted=false;
}

void CDC1394Camera::SetROI()
{
    dc1394_format7_set_roi(mCam,mMode,
                           (dc1394color_coding_t)DC1394_QUERY_FROM_CAMERA, DC1394_QUERY_FROM_CAMERA,
                           DC1394_QUERY_FROM_CAMERA, DC1394_QUERY_FROM_CAMERA,
                           DC1394_QUERY_FROM_CAMERA, DC1394_QUERY_FROM_CAMERA);
}

void CDC1394Camera::SetFrameRate(dc1394framerate_t pRate)
{
    dc1394_video_set_framerate(mCam,pRate);
}

void CDC1394Camera::SetISOSpeed(dc1394speed_t pSpeed)
{
    dc1394_video_set_iso_speed(mCam,pSpeed);
}

void CDC1394Camera::SetMode(dc1394video_mode_t pMode)
{
    dc1394_video_set_mode(mCam,pMode);
    mMode=pMode;
}

void CDC1394Camera::SetBayerFilter(dc1394color_filter_t pFilter)
{
    mBayerFilter=pFilter;
}

void CDC1394Camera::SetWhiteBalance(int p1,int p2)
{
    dc1394_feature_whitebalance_set_value(mCam,p1,p2);
}

void CDC1394Camera::SetImageSize(const CSize_i &pSize,dc1394video_mode_t pMode)
{
    dc1394_format7_set_image_size(mCam, pMode, pSize.x,pSize.y);
}

void CDC1394Camera::SetImagePos(const CPoint_i &pPos,dc1394video_mode_t pMode)
{
    dc1394_format7_set_image_position(mCam, pMode, pPos.x,pPos.y);
}

static const dc1394feature_t sFeatureMap[]=
{
    DC1394_FEATURE_BRIGHTNESS,
    DC1394_FEATURE_EXPOSURE,
    DC1394_FEATURE_SHARPNESS,
    DC1394_FEATURE_WHITE_BALANCE,
    DC1394_FEATURE_HUE,
    DC1394_FEATURE_SATURATION,
    DC1394_FEATURE_GAMMA,
    DC1394_FEATURE_SHUTTER,
    DC1394_FEATURE_GAIN,
    DC1394_FEATURE_IRIS,
    DC1394_FEATURE_FOCUS,
    DC1394_FEATURE_TEMPERATURE,
    DC1394_FEATURE_TRIGGER,
    DC1394_FEATURE_TRIGGER_DELAY,
    DC1394_FEATURE_WHITE_SHADING
};

void CDC1394Camera::SetFeature(int pFeature,unsigned int pValue)
{
    dc1394_feature_set_value(mCam,sFeatureMap[pFeature],pValue);
}

void CDC1394Camera::SetFeatureAbs(int pFeature,double pValue)
{
    dc1394_feature_set_absolute_value(mCam,sFeatureMap[pFeature],pValue);
}

void CDC1394Camera::SetFeature(int pFeature,unsigned int p1,unsigned int p2)
{
    //    if(pFeature==NCam::FEATURE_WHITEBALANCE)
    //        dc1394_feature_whitebalance_set_value(mP->mDC1394Camera,p1,p2);
}

void CDC1394Camera::SetFeature(int pFeature,unsigned int p1,unsigned int p2,
                               unsigned int p3)
{
    //    if(pFeature==NCam::FEATURE_WHITESHADING)
    //        dc1394_feature_whiteshading_set_value(mP->mDC1394Camera,p1,p2,p3);
}

void CDC1394Camera::SetFeatureMode(int pFeature,int pMode)
{
    /*
    switch(pMode)
    {
        case NCam::MODE_NONE:
            break;
        case NCam::MODE_OFF:
            dc1394_feature_set_mode(mP->mDC1394Camera,sFeatureMap[pFeature],DC1394_FEATURE_MODE_MANUAL);
            dc1394_feature_set_power(mP->mDC1394Camera,sFeatureMap[pFeature],DC1394_OFF);
            break;
        case NCam::MODE_MANUAL:
            dc1394_feature_set_power(mP->mDC1394Camera,sFeatureMap[pFeature],DC1394_ON);
            dc1394_feature_set_mode(mP->mDC1394Camera,sFeatureMap[pFeature],DC1394_FEATURE_MODE_MANUAL);
            break;
        case NCam::MODE_AUTO:
            dc1394_feature_set_power(mP->mDC1394Camera,sFeatureMap[pFeature],DC1394_ON);
            dc1394_feature_set_mode(mP->mDC1394Camera,sFeatureMap[pFeature],DC1394_FEATURE_MODE_AUTO);
            break;
    }
    */
}

uint32_t CDC1394Camera::GetControlRegister(off_t pOffset)
{
    uint32_t lValue=0;
    dc1394_get_control_register(mCam,pOffset,&lValue);
    return lValue;
}

void CDC1394Camera::SetControlRegister(off_t pOffset,uint32_t pValue)
{
    dc1394_set_control_register(mCam,pOffset,pValue);
}

void CDC1394Camera::DequeueFrame(dc1394video_frame_t **pFrame)
{
    while(true)
    {
        mDC->mMutex.lock();
        if(dc1394_capture_dequeue(mCam,DC1394_CAPTURE_POLICY_POLL,pFrame) != DC1394_SUCCESS)
            throw XHardware("unable to capture a frame");
        mDC->mMutex.unlock();
        if(*pFrame&&!dc1394_capture_is_frame_corrupt(mCam,*pFrame)) return;
        sleep(chrono::microseconds(200));
    }
}

void CDC1394Camera::EnqueueFrame(dc1394video_frame_t *pFrame)
{
    mDC->mMutex.lock();
    dc1394_capture_enqueue(mCam,pFrame);
    mDC->mMutex.unlock();
}

void CDC1394Camera::PrintInfo(io::CTextWriter &pWriter)
{
    pWriter << "\n******************** camera info *********************\n\n" << nFlush;
    dc1394_camera_print_info(mCam,stderr);

    pWriter << "\n******************** feature set *********************\n\n" << nFlush;
    dc1394_feature_print_all(&mFeatureSet,stderr);

    pWriter << "\n******************************************************\n\n";
    pWriter << "   iso speed: " << (int)mISOSpeed << "\n";
    pWriter << "   mode: " << (int)mMode << "\n";
    pWriter << "   framerate: " << (int)mFrameRate << "\n";
    pWriter << "   supported modes: \n";
    for(int i=0;i<mModes.num;i++)
    {
        pWriter << "      " << (int)mModes.modes[i] << "\n";
        if(mModes.modes[i]>=DC1394_VIDEO_MODE_FORMAT7_MIN&&mModes.modes[i]<=DC1394_VIDEO_MODE_FORMAT7_MAX)
        {
            pWriter << "         format7\n";
        }
        else
        {
            pWriter << "         supported framerates: \n";
            dc1394framerates_t lFrameRates;
            if(dc1394_video_get_supported_framerates(mCam,mModes.modes[i],&lFrameRates)!=DC1394_SUCCESS)
                throw XHardware("unable to get supported framerates");
            for(int i=0;i<lFrameRates.num;i++)
            {
                pWriter << "             " << (int)lFrameRates.framerates[i] << "\n";
            }
        }
    }
}

/*namespace blackbox*/ } /*namespace buola*/ }

#endif
